#include "stm32f10x.h"
#include "lx255.h"
#include "usart.h"
#include "delay.h"

#define USART USART3

int r1=500; int r2=500; int r3=500; int r4=500; int r5=500; 
int speed=5; int flash=10;int isRoll1=0;int isRoll2=0;int isShift=0;

void  control_Init(void){
	GPIO_InitTypeDef GPIO_InitStruct;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_Out_PP;
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_4;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_InitStruct);
	
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_5;
	GPIO_Init(GPIOB,&GPIO_InitStruct);
}

void roll(int ID,int r,int time){
	char dat[10]={0x55,0x55,8,3,1,0,0,0,0,0};
	dat[7]=(char)ID; dat[8]=r; dat[9]=r>>8; dat[5]=time; dat[6]=time>>8;
	USART_Send(USART,dat,10);
	//USART_Send(USART2,dat,10);
}

#include <math.h>
void getCmd(char* cmd,int len){
	if (cmd[2]=='n'){
		if(cmd[3]=='u'||cmd[3]=='x') isRoll1=0;
		if(cmd[3]=='l'||cmd[3]=='r') isRoll2=0;
		if(cmd[3]=='a'||cmd[3]=='d') {isShift=0;isRoll2=0;}
	}
	else{
		if(cmd[2]=='u') isRoll1=-1;
		else if(cmd[2]=='x') isRoll1=1;
		if(cmd[2]=='l') {isShift=1;isRoll2=1;}
		else if(cmd[2]=='r') {isShift=1;isRoll2=-1;}
		if(cmd[2]=='a') {isShift=-1;isRoll2=1;}
		else if(cmd[2]=='d') {isShift=-1;isRoll2=-1;}
	}
}

void getCmdEngine(char* cmd,int len){
	
}

void changeNum(int *num,int isRoll)
{
	*num+=(speed*isRoll);
	*num=*num>1000?1000:*num;
	*num=*num<0?0:*num;
}

void move(void){
	while (1){
		//if(isRoll2||isRoll1) delay_ms(flash);
		if(isRoll1){
			roll(1,500,000);
			roll(3,500,000);
			roll(5,r5,000);
			changeNum(&r1,isRoll1);
			changeNum(&r3,isRoll1);
			changeNum(&r5,isRoll1);
//			r1+=(speed*isRoll1);
//			r1=r1>1000?1000:r1;
//			r1=r1<0?0:r1;
//			r3+=(speed*isRoll1);
//			r3=r3>1000?1000:r3;
//			r3=r3<0?0:r3;
//			r5+=(speed*isRoll1);
//			r5=r5>1000?1000:r5;
//			r5=r5<0?0:r5;
		}
		if(isRoll2){
			roll(2,r2,000);
			roll(4,r4,000);
			changeNum(&r2,isRoll2);
			changeNum(&r4,isRoll2*isShift);
//			r2-=(speed*isRoll2);
//			r2=r2>1000?1000:r2;
//			r2=r2<0?0:r2;
//			r4-=(speed*isRoll2*isShift);
//			r4=r4>1000?1000:r4;
//			r4=r4<0?0:r4;
		}
	}
}
	


